/*!
  @file   VectorTypes.cpp
  @author Jared Marsau
  @author Hamza Hutchinson
  @date   03/01/2009
  @brief  
    This file constains the implementation for 2, 3, and 4 dimensional vectors.
*/

#include "VectorTypes.hpp"
#include "RichAssert.hpp"

#include <cmath> //sqrt

namespace VectorTypes
{
  //////////////////////////////////////////////////////////////////////////////
  //Vector2D
	
  /*!
  @brief
    Default contstructor does nothing.
  */
  Vector2D::Vector2D()
  {}
	
  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector2D::Vector2D(const float* arrayOfTwoFloats)
  : x(arrayOfTwoFloats[0]), y(arrayOfTwoFloats[1])
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector2D::Vector2D(float xVal, float yVal)
  : x(xVal), y(yVal)
  {}

  /*!
  @brief
    Assigns the values in rhs to *this.
  */
  Vector2D& Vector2D::operator = (const Vector2D& rhs)
  {
    x = rhs.x;
    y = rhs.y;

    return *this;
  }
	
  /*!
  @brief
    Componentwise addition of rhs to *this.
  */
  Vector2D& Vector2D::operator += (const Vector2D& rhs)
  {
    x += rhs.x;
    y += rhs.y;

    return *this;
  }
	
  /*!
  @brief
    Componentwise subtraction of rhs from *this.
  */
  Vector2D& Vector2D::operator -= (const Vector2D& rhs)
  {
    x -= rhs.x;
    y -= rhs.y;

    return *this;
  }
	
  /*!
  @brief
    Scales *this by rhs.
  */
  Vector2D& Vector2D::operator *= (float rhs)
  {
    x *= rhs;
    y *= rhs;

    return *this;
  }
	
  /*!
  @brief
    Scales *this by 1 / rhs.
  */
  Vector2D& Vector2D::operator /= (float rhs)
  {
    x /= rhs;
    y /= rhs;

    return *this;
  }
	
  /*!
  @brief
    Returns the negative vector of *this.
  */
  Vector2D Vector2D::operator - () const
  {
    return Vector2D(-x, -y);
  }
	
  /*!
  @brief
    Componentwise addition of rhs and *this.
  */
  Vector2D Vector2D::operator + (const Vector2D& rhs) const
  {
    return Vector2D(x + rhs.x, y + rhs.y);
  }
	
  /*!
  @brief
    Componentwise subtraction of rhs and *this.
  */
  Vector2D Vector2D::operator - (const Vector2D& rhs) const
  {
    return Vector2D(x - rhs.x, y - rhs.y);
  }
	
  /*!
  @brief
    Index into this vector.
  */
  float& Vector2D::operator [] (unsigned index)
  {
    assert(x < 2);
    return (&x)[index];
  }
	
  /*!
  @brief
    Index into this vector.
  */
  const float& Vector2D::operator [] (unsigned index) const
  {
    assert(x < 2);
    return (&x)[index];
  }
	
  /*!
  @brief
    The dot product of rhs and *this.
  */
  float Vector2D::operator * (const Vector2D& rhs) const
  {
    return ((x * rhs.x) + (y * rhs.y));
  }
	
  /*!
  @brief
    Returns *this scaled by rhs.
  */
  Vector2D Vector2D::operator * (float rhs) const
  {
    return Vector2D(x * rhs, y * rhs);
  }
	
  /*!
  @brief
    Returns *this scaled by 1 / rhs.
  */
  Vector2D Vector2D::operator / (float rhs) const
  {
    return Vector2D(x / rhs, y / rhs);
  }
	
  /*!
  @brief
    Returns true if the components of *this are equal to the components of rhs.
  */
  bool Vector2D::operator == (const Vector2D& rhs) const
  {
    return ((x == rhs.x) && (y == rhs.y));
  }
	
  /*!
  @brief
    Returns true if the components of *this are not equal to the components of
      rhs.
  */
  bool Vector2D::operator != (const Vector2D& rhs) const
  {
    return ((x != rhs.x) || (y != rhs.y));
  }
	
  /*!
  @brief
    Sets the components of *this to the given values.
  */
  void Vector2D::Set (float xVal, float yVal)
  {
    x = xVal;
    y = yVal;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  float* Vector2D::GetArrayPtr ()
  {
    return &x;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  const float* Vector2D::GetArrayPtr () const
  {
    return &x;
  }
	
  /*!
  @brief
    Componentwise assignment of source to *this.
  */
  void Vector2D::Set (const Vector2D& source)
  {
    x = source.x;
    y = source.y;
  }
	
  /*!
  @brief
    Normalizes this vector.
  */
  void Vector2D::Normalize ()
  {
    float len = Length();
    assert(len != 0); //cover our bases
    x        /= len;
    y        /= len;
  }
	
  /*!
  @brief
    Returns the length of the vector.
  */
  float Vector2D::Length () const
  {
    return sqrt((x * x) + (y * y));
  }
	
  /*!
  @brief
    Returns the length squared of the vector.
  */
  float Vector2D::LengthSquared () const
  {
    return ((x * x) + (y * y));
  }
	
  /*!
  @brief
    Returns a normalized copy of *this.
  */
  Vector2D Vector2D::Normalized () const
  {
    float len = Length();
    return Vector2D(x / len, y / len);
  }
	
  /*!
  @brief
    Returns true the length squared of *this is less than tolerance.
  */
  bool Vector2D::IsZeroLength (float tolerance) const
  {
    return (LengthSquared() < tolerance);
  }
	
  /*!
  @brief
    Returns the rhs vector scaled by lhs.
  */
  Vector2D operator * (float lhs, const Vector2D& rhs)
  {
    return Vector2D(lhs * rhs.x, lhs * rhs.y);
  }




  //////////////////////////////////////////////////////////////////////////////
  //Vector3D
  
  /*!
  @brief
    Default contstructor does nothing.
  */
  Vector3D::Vector3D()
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector3D::Vector3D(const float* arrayOfThreeFloats)
  : x(arrayOfThreeFloats[0]), y(arrayOfThreeFloats[1]), z(arrayOfThreeFloats[2])
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector3D::Vector3D(float xVal, float yVal, float zVal)
  : x(xVal), y(yVal), z(zVal)
  {}
	
  /*!
  @brief
    Assigns the values in rhs to *this.
  */
  Vector3D& Vector3D::operator =  (const Vector3D& rhs)
  {
    x = rhs.x;
    y = rhs.y;
    z = rhs.z;

    return *this;
  }
	
  /*!
  @brief
    Componentwise addition of rhs to *this.
  */
  Vector3D& Vector3D::operator += (const Vector3D& rhs)
  {
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;

    return *this;
  }
	
  /*!
  @brief
    Componentwise subtraction of rhs from *this.
  */
  Vector3D& Vector3D::operator -= (const Vector3D& rhs)
  {
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;

    return *this;
  }
	
  /*!
  @brief
    Scales *this by rhs.
  */
  Vector3D& Vector3D::operator *= (float rhs)
  {
    x *= rhs;
    y *= rhs;
    z *= rhs;

    return *this;
  }

  /*!
  @brief
    Scales *this by 1 / rhs.
  */
  Vector3D& Vector3D::operator /= (float rhs)
  {
    x /= rhs;
    y /= rhs;
    z /= rhs;

    return *this;
  }

  /*!
  @brief
    Returns the negative vector of *this.
  */
  Vector3D Vector3D::operator - () const
  {
    return Vector3D(-x, -y, -z);
  }

  /*!
  @brief
    Componentwise addition of rhs and *this.
  */
  Vector3D Vector3D::operator + (const Vector3D& rhs) const
  {
    return Vector3D(x + rhs.x, y + rhs.y, z + rhs.z);
  }

  /*!
  @brief
    Componentwise subtraction of rhs and *this.
  */
  Vector3D Vector3D::operator - (const Vector3D& rhs) const
  {
    return Vector3D(x - rhs.x, y - rhs.y, z - rhs.z);
  }

  /*!
  @brief
    Index into this vector.
  */
  float& Vector3D::operator [] (unsigned index)
  {
    assert(x < 3);
    return (&x)[index];
  }

  /*!
  @brief
    Index into this vector.
  */
  const float& Vector3D::operator [] (unsigned index) const
  {
    assert(x < 3);
    return (&x)[index];
  }
	
  /*!
  @brief
    The dot product of rhs and *this.
  */
  float Vector3D::operator * (const Vector3D& rhs) const
  {
    return ((x * rhs.x) + (y * rhs.y) + (z * rhs.z));
  }
	
  /*!
  @brief
    The cross product of *this with rhs.
  */
  Vector3D Vector3D::operator ^ (const Vector3D& rhs) const
  {
    return Vector3D(y * rhs.z - z * rhs.y, z * rhs.x - x * rhs.z, x * rhs.y - y * rhs.x);
  }

  /*!
  @brief
    Returns *this scaled by rhs.
  */
  Vector3D Vector3D::operator * (float rhs) const
  {
    return Vector3D(x * rhs, y * rhs, z * rhs);
  }
	
  /*!
  @brief
    Returns *this scaled by 1 / rhs.
  */
  Vector3D Vector3D::operator / (float rhs) const
  {
    return Vector3D(x / rhs, y / rhs, z / rhs);
  }
	
  /*!
  @brief
    Returns true if the components of *this are equal to the components of rhs.
  */
  bool Vector3D::operator == (const Vector3D& rhs) const
  {
    return ((x == rhs.x) && (y == rhs.y) && (z == rhs.z));
  }
	
  /*!
  @brief
    Returns true if the components of *this are not equal to the components of
     rhs.
  */
  bool Vector3D::operator != (const Vector3D& rhs) const
  {
    return ((x != rhs.x) || (y != rhs.y) || (z != rhs.z));
  }
	
  /*!
  @brief
    Sets the components of *this to the given values.
  */
  void Vector3D::Set (float xVal, float yVal, float zVal)
  {
    x = xVal;
    y = yVal;
    z = zVal;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  float* Vector3D::GetArrayPtr ()
  {
    return &x;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  const float* Vector3D::GetArrayPtr () const
  {
    return &x;
  }
	
  /*!
  @brief
    Componentwise assignment of source to *this.
  */
  void Vector3D::Set (const Vector3D& source)
  {
    x = source.x;
    y = source.y;
    z = source.z;
  }
	
  /*!
  @brief
    Normalizes this vector.
  */
  void Vector3D::Normalize ()
  {
    float len = Length();
    assert(len != 0); //cover our bases
    x        /= len;
    y        /= len;
    z        /= len;
  }
	
  /*!
  @brief
    Returns the length of the vector.
  */
  float Vector3D::Length () const
  {
    return sqrt((x * x) + (y * y) + (z * z));
  }
	
  /*!
  @brief
    Returns the length squared of the vector.
  */
  float Vector3D::LengthSquared () const
  {
    return ((x * x) + (y * y) + (z * z));
  }
	
  /*!
  @brief
    Returns a normalized copy of *this.
  */
  Vector3D Vector3D::Normalized () const
  {
    float len = Length();
    return Vector3D(x / len, y / len, z / len);
  }
	
  /*!
  @brief
    Returns true the length squared of *this is less than tolerance.
  */
  bool Vector3D::IsZeroLength (float tolerance) const
  {
    return (LengthSquared() < tolerance);
  }
	
  /*!
  @brief
    Returns the rhs vector scaled by lhs.
  */
  Vector3D operator * (float lhs, const Vector3D& rhs)
  {
    return Vector3D(lhs * rhs.x, lhs * rhs.y, lhs * rhs.z);
  }




  //////////////////////////////////////////////////////////////////////////////
	//Vector4D
  
  /*!
  @brief
    Default contstructor does nothing.
  */
  Vector4D::Vector4D()
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector4D::Vector4D(const float* arrayOfFourFloats)
  : x(arrayOfFourFloats[0]), y(arrayOfFourFloats[1]), z(arrayOfFourFloats[2]), w(arrayOfFourFloats[3])
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector4D::Vector4D(const Vector3D& xyzVal, float wVal)
  : x(xyzVal.x), y(xyzVal.y), z(xyzVal.z), w(wVal)
  {}

  /*!
  @brief
    Initializes the vector with the given data.
  */
  Vector4D::Vector4D(float xVal, float yVal, float zVal, float wVal)
  : x(xVal), y(yVal), z(zVal), w(wVal)
  {}
	
  /*!
  @brief
    Assigns the values in rhs to *this.
  */
  Vector4D& Vector4D::operator =  (const Vector4D& rhs)
  {
    x = rhs.x;
    y = rhs.y;
    z = rhs.z;
    w = rhs.w;

    return *this;
  }
	
  /*!
  @brief
    Componentwise addition of rhs to *this.
  */
  Vector4D& Vector4D::operator += (const Vector4D& rhs)
  {
    x += rhs.x;
    y += rhs.y;
    z += rhs.z;
    w += rhs.w;

    return *this;
  }
	
  /*!
  @brief
    Componentwise subtraction of rhs from *this.
  */
  Vector4D& Vector4D::operator -= (const Vector4D& rhs)
  {
    x -= rhs.x;
    y -= rhs.y;
    z -= rhs.z;
    w -= rhs.w;

    return *this;
  }
	
  /*!
  @brief
    Scales *this by rhs.
  */
  Vector4D& Vector4D::operator *= (float rhs)
  {
    x *= rhs;
    y *= rhs;
    z *= rhs;
    w *= rhs;

    return *this;
  }

  /*!
  @brief
    Scales *this by 1 / rhs.
  */
  Vector4D& Vector4D::operator /= (float rhs)
  {
    x /= rhs;
    y /= rhs;
    z /= rhs;
    w /= rhs;

    return *this;
  }

  /*!
  @brief
    Returns the negative vector of *this.
  */
  Vector4D Vector4D::operator - () const
  {
    return Vector4D(-x, -y, -z, -w);
  }

  /*!
  @brief
    Componentwise addition of rhs and *this.
  */
  Vector4D Vector4D::operator + (const Vector4D& rhs) const
  {
    return Vector4D(x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
  }

  /*!
  @brief
    Componentwise subtraction of rhs and *this.
  */
  Vector4D Vector4D::operator - (const Vector4D& rhs) const
  {
    return Vector4D(x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
  }

  /*!
  @brief
    Index into this vector.
  */
  float& Vector4D::operator [] (unsigned index)
  {
    assert(x < 4);
    return (&x)[index];
  }

  /*!
  @brief
    Index into this vector.
  */
  const float& Vector4D::operator [] (unsigned index) const
  {
    assert(x < 4);
    return (&x)[index];
  }
	
  /*!
  @brief
    The dot product of rhs and *this.
  */
  float Vector4D::operator * (const Vector4D& rhs) const
  {
    return ((x * rhs.x) + (y * rhs.y) + (z * rhs.z) + (w * rhs.w));
  }

  /*!
  @brief
    Returns *this scaled by rhs.
  */
  Vector4D Vector4D::operator * (float rhs) const
  {
    return Vector4D(x * rhs, y * rhs, z * rhs, w * rhs);
  }
	
  /*!
  @brief
    Returns *this scaled by 1 / rhs.
  */
  Vector4D Vector4D::operator / (float rhs) const
  {
    return Vector4D(x / rhs, y / rhs, z / rhs, w / rhs);
  }
	
  /*!
  @brief
    Returns true if the components of *this are equal to the components of rhs.
  */
  bool Vector4D::operator == (const Vector4D& rhs) const
  {
    return ((x == rhs.x) && (y == rhs.y) && (z == rhs.z) && (w == rhs.w));
  }
	
  /*!
  @brief
    Returns true if the components of *this are not equal to the components of
     rhs.
  */
  bool Vector4D::operator != (const Vector4D& rhs) const
  {
    return ((x != rhs.x) || (y != rhs.y) || (z != rhs.z) || (w != rhs.w));
  }

  /*!
  @brief
    Divides each component of the vector by w.
  */
  void Vector4D::HomogeneousDivide()
  {
    x /= w;
    y /= w;
    z /= w;
    w  = 1;
  }

  /*!
  @brief
    Sets the components of *this to the given values.
  */
  void Vector4D::Set (float xVal, float yVal, float zVal, float wVal)
  {
    x = xVal;
    y = yVal;
    z = zVal;
    w = wVal;
  }
	
  /*!
  @brief
    Componentwise assignment of source to *this.
  */
  void Vector4D::Set (const Vector4D& source)
  {
    x = source.x;
    y = source.y;
    z = source.z;
    w = source.w;
  }

  /*!
  @brief
    Sets the components of *this to the given values.
  */
  void Vector4D::Set(const Vector3D& xyzVal, float wVal)
  {
    x = xyzVal.x;
    y = xyzVal.y;
    z = xyzVal.z;
    w = wVal;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  float* Vector4D::GetArrayPtr ()
  {
    return &x;
  }
	
  /*!
  @brief
    Returns a pointer to the vector elements in contiguous memory.
  */
  const float* Vector4D::GetArrayPtr () const
  {
    return &x;
  }
	
  /*!
  @brief
    Normalizes this vector.
  */
  void Vector4D::Normalize ()
  {
    float len = Length();
    assert(len != 0); //cover our bases
    x        /= len;
    y        /= len;
    z        /= len;
    w        /= len;
  }
	
  /*!
  @brief
    Returns the length of the vector.
  */
  float Vector4D::Length () const
  {
    return sqrt((x * x) + (y * y) + (z * z) + (w * w));
  }
	
  /*!
  @brief
    Returns the length squared of the vector.
  */
  float Vector4D::LengthSquared () const
  {
    return ((x * x) + (y * y) + (z * z) + (w * w));
  }
	
  /*!
  @brief
    Returns a normalized copy of *this.
  */
  Vector4D Vector4D::Normalized () const
  {
    float len = Length();
    return Vector4D(x / len, y / len, z / len, z / len);
  }
	
  /*!
  @brief
    Returns true the length squared of *this is less than tolerance.
  */
  bool Vector4D::IsZeroLength (float tolerance) const
  {
    return (LengthSquared() < tolerance);
  }
	
  /*!
  @brief
    Returns the rhs vector scaled by lhs.
  */
  Vector4D operator * (float lhs, const Vector4D& rhs)
  {
    return Vector4D(lhs * rhs.x, lhs * rhs.y, lhs * rhs.z, lhs * rhs.w);
  }


} //namespace VectorTypes
